/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_filter.h"

/**
 * @description: Y=A*X+(1-A)*Y0 -> Y=Y0+((X-Y0)*a)/1000; 0<a<1000
 * @return {*}
 * @param {int} a 扩大了1000倍的系数，避免浮点数计算
 * @param {int} x 输入值
 * @param {int} y 上一次输出值
 */
int32_t lowPassFilter1Order(int a, int x, int y)
{
	return y + ((x - y) * a) / 1000;
}

int16_t filterKalman1DimCalc(KF1DimObj* kf, int16_t input)
{
    int32_t enlarge = input * 1024;
    kf->Now_P = kf->LastP + kf->Q;
    kf->Kg = kf->Now_P * 1024 / (kf->Now_P + kf->R);
    kf->out += kf->Kg * (enlarge - kf->out) / 1024;
    kf->LastP = (1024 - kf->Kg) * kf->Now_P / 1024;
    return kf->out / 1024;
}